#include "Interpolater.h"

Interpolater::Interpolater(QObject *parent) : QObject(parent)
{
    stopped = false;
    keepRunning = false;
}

void Interpolater::interpolate(QList<BaseNode *> nodeList)
{
    for (int i = 0; i < nodeList.size(); ++i)
    {
        QPointF start, end;
        if (nodeList.at(i)->getKind() == PK_LINE)
        {
            start = QPointF();
            end = nodeList.at(i)->getEndPoint();
        }
        else
        {
            start = QPointF();
            end = nodeList.at(i)->getEndPoint();
        }
        interpolateOneLine(start, end);
    }
    if (keepRunning) // keep emitting 0 output
    {
        // to let the filters output all their responces
        QEventLoop loop;
        while (!stopped)
        {
            loop.processEvents();
            QThread::usleep(_period * 500);
            emit output(0.0, 0.0);
        }
    }
    keepRunning = false;
    stopped = false;
}

void Interpolater::setPeriod(int period)
{
    // unit: ms
    _period = period;
}

void Interpolater::setFeedRate(int feedrate)
{
    // unit: mm/min
    _feedrate = feedrate;
}

void Interpolater::interpolateOneLine(QPointF start, QPointF end)
{
    qreal length = hypot(start.x() - end.x(), start.y() - end.y()) * 1000;
    qreal feedInPeriod = _feedrate / 60.0 * _period; // unit: um /ms
    // Using DDA interpolation method
    int m = (int)(1.0 * length / feedInPeriod); // accumulation times
    qreal residual = length - m * feedInPeriod;
    double sumX = 0, sumY = 0; // accumulation of X and Y
    int endX, endY; // projection on each axis
    endX = (end.x() - start.x()) * 1000;
    endY = (end.y() - start.y()) * 1000;
    QEventLoop loop;
    for (int count = m; count > 0; --count)
    {
        loop.processEvents();
        QThread::usleep(_period * 500);
        if (stopped)
        {
            break;
        }
        double feedX, feedY;
        sumX += endX;
        if (sumX >= fabs(endX) * fabs(endX) / length)
        {
            sumX -= fabs(endX) * fabs(endX) / length;
            feedX = feedInPeriod * fabs(endX) / length;
        }
        else if (sumX <= -fabs(endX) * fabs(endX) / length)
        {
            sumX += fabs(endX) * fabs(endX) / length;
            feedX = -feedInPeriod * fabs(endX) / length;
        }
        else
        {
            feedX = 0;
        }
        sumY += endY;
        if (sumY >= fabs(endY) * fabs(endY) / length)
        {
            sumY -= fabs(endY) * fabs(endY) / length;
            feedY = feedInPeriod * fabs(endY) / length;
        }
        else if (sumY <= -fabs(endY) * fabs(endY) / length)
        {
            sumY += fabs(endY) * fabs(endY) / length;
            feedY = -feedInPeriod * fabs(endY) / length;
        }
        else
        {
            feedY = 0;
        }
        emit output(feedX / 1000.0, feedY / 1000.0);
//        qDebug() << feedX / 1000.0 << feedY / 1000.0;
    }
    emit output(residual * endX / length / 1000.0,
                residual * endY / length / 1000.0);
}

void Interpolater::stop()
{
    stopped = true;
}

void Interpolater::setKeepRunning()
{
    keepRunning = true;
}
